#include "nav.h"
#include <stdbool.h>
#include <math.h>


/********************************************

Navigational routines to be used in RC autopilot software


double radians(double deg) 
double degrees(double rad) 

unsigned int get_gps_dist(float flat1, float flon1, float flat2, float flon2)
int get_gps_course(float flat1, float flon1, float flat2, float flon2)


Simplified versions. OK precision under 10 km
int get_gps_course2(float flat1, float flon1, float flat2, float flon2)
unsigned int get_gps_dist2(float flat1, float flon1, float flat2, float flon2)
 


**********************************************/



t_waypoint wp[10];


static int no_of_waypoints=0;
unsigned int home_set=0;;

static double wp_course;
static char lastwp=0;
static char nextwp=0;
static double distance_to_wp=0;
static double course_to_wp=0;
static double xte=0;
static double i_xte=0;
static double d_xte=0;
static double wp_cog_error;
static double cog_error;

static double cog_target;
static double tempturn;
static int max_xte_nav_angle=75;
static double KP=1.0;
static double KI=0.2;
static double KD=0.5;
static double max_turnrate=40;

static double turnrate=0;
static double tempcog=0;	


// Global variables

gps_data last_pos;

char getnextwp(){
  return nextwp;

}


double getwpcourse(){
  return wp_course;

}



double radians(double deg) {
  double rad;
  rad=deg*M_PI/180;
  return rad;

}

double degrees(double rad) {
  double deg;
  deg=rad*180/M_PI;
  return deg;

}


double limitdeg(double deg) {
 if (deg>=360) 
      deg=deg-360;
 if (deg<0) 
      deg=360+deg;
  return deg;
}

double limiterror(double deg) {
 if (deg>180) 
      deg=deg-360;
 if (deg<-180) 
      deg=deg+180;
  return deg;
}


double get_gps_course2(float flat1, float flon1, float flat2, float flon2)
{
float calc;
float bear_calc;
float x = 69.1 * (flat2 - flat1);
float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);

calc=atan2(y,x);
bear_calc= degrees(calc);

if(bear_calc<=1){
bear_calc=360+bear_calc; }
return bear_calc;
}

double get_gps_dist2(float flat1, float flon1, float flat2, float flon2)
{
float x = 69.1 * (flat2 - flat1);
float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);
return (float)sqrt((float)(x*x) + (float)(y*y))*1609.344;
}


 /*************************************************************************
 //Function to calculate the course between two waypoints
 //I'm using the real formulas--no lookup table fakes!
 *************************************************************************/ 
 double get_gps_course(float flat1, float flon1, float flat2, float flon2)
 {
 float calc;
 float calc2;
 float bear_calc;
 float diflon;
 //I've to spplit all the calculation in several steps.  If i try it to do it in a single line the arduino will explode.
	 flat1=radians(flat1);
	 flat2=radians(flat2);

	 diflon=radians((flon2)-(flon1));
	 
	 calc=sin(diflon)*cos(flat2);
	 calc2=cos(flat1)*sin(flat2)-sin(flat1)*cos(flat2)*cos(diflon);

	 calc=atan2(calc,calc2);

     bear_calc= degrees(calc);

   if(bear_calc<=1){bear_calc=360+bear_calc; }
   return bear_calc;

 }

 /*************************************************************************
 //Function to calculate the distance between two waypoints
 //I'm using the real formulas
 *************************************************************************/ 
  double get_gps_dist(float flat1, float flon1, float flat2, float flon2)
 {
 float dist_calc=0;
 float dist_calc2=0;
 float diflat=0;
 float diflon=0;
 
 //I've to spplit all the calculation in several steps.  If i try it to do it in a single line the arduino will explode.
	 diflat=radians(flat2-flat1);
	 flat1=radians(flat1);
	 flat2=radians(flat2);

	  diflon=radians((flon2)-(flon1));

  dist_calc =  (sin(diflat/2.0)*sin(diflat/2.0)); 
  dist_calc2= cos(flat1);
  dist_calc2*=cos(flat2);
  dist_calc2*=sin(diflon/2.0);
  dist_calc2*=sin(diflon/2.0);
  dist_calc +=dist_calc2;

  dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));

  dist_calc*=6371000.0; //Converting to meters, i love the metric system.. =)
  return dist_calc;
 }




/*************************************

wp_next 

moves on to next wp in list. if no more waypoints 
exists it starts over with number one. 

**************************************/


char wp_next(int next_wp) {
   lastwp=nextwp;
   if (nextwp==no_of_waypoints) 
     nextwp=1;
   else {
     nextwp++;
   }
   wp_course=get_gps_course(wp[lastwp].la,wp[lastwp].lo,wp[nextwp].la,wp[nextwp].lo);
   distance_to_wp=get_gps_dist(wp[lastwp].la,wp[lastwp].lo,wp[nextwp].la,wp[nextwp].lo);
   i_xte=0;
   d_xte=0;
   return nextwp;
}  // wp_next


/*************************************

wp_goto 

goto wp number.wp_no

in wp_no <= number_of_waypoints then loads that wp
else returns false and do nothing

**************************************/

char wp_goto(int wp_no) {

  return 0;
}


/*************************************

wp_go_home

Activates wp[0] and returns 1
if wp[0] is not set then wp_go_home returns -1

**************************************/

char wp_go_home() {

  if (home_set==1) {
    lastwp=nextwp;
    nextwp=0;
    wp_course=get_gps_course(wp[lastwp].la,wp[lastwp].lo,wp[nextwp].la,wp[nextwp].lo);
 //   distance_to_wp=get_gps_dist(last_pos.la,last_pos.lo,wp[nextwp].la,wp[nextwp].lo);
    i_xte=0;
    d_xte=0;
    return 1;
  }
  else
    return 0;
}  // wp_go_home


/*************************************

wp_add 

adds a waypoint to the waypoint list and return new number of waypoints

**************************************/

char wp_add(double la,double lo, int alt, int arrival_dist) {
  no_of_waypoints++;
  wp[no_of_waypoints].la=la;
  wp[no_of_waypoints].lo=lo;
  wp[no_of_waypoints].alt=alt;
  wp[no_of_waypoints].arrival_dist=arrival_dist;
  return no_of_waypoints;
}  // wp_add



/*************************************

wp_set_home 

sets wp[0] to home coordinates

**************************************/

void wp_set_home(double la,double lo, int alt, int arrival_dist) {
  wp[0].la=la;
  wp[0].lo=lo;
  wp[0].alt=alt;
  wp[0].arrival_dist=arrival_dist;
  home_set=1;
}  // wp_set_home



/***********************************************

Main navigation routine

makes plane follow a line between lastwp and next wp

double new_la,double new_lo,double new_cog,double new_sog

************************************************/



double navigate() {
	tempcog=last_pos.cog;
	distance_to_wp=get_gps_dist(last_pos.la,last_pos.lo,wp[nextwp].la,wp[nextwp].lo);
	course_to_wp=get_gps_course(last_pos.la,last_pos.lo,wp[nextwp].la,wp[nextwp].lo);

	wp_cog_error = course_to_wp - wp_course;
 	wp_cog_error = limiterror(wp_cog_error);
 
 	if ((wp_cog_error>max_xte_nav_angle) || (wp_cog_error<-max_xte_nav_angle)) {
	    //nav_flag:=0;
    	cog_target=course_to_wp;
	}
	else { 
 	  	d_xte=xte;
		i_xte=i_xte+xte;
    	xte=distance_to_wp*sin(radians(wp_cog_error));
    	d_xte=xte-d_xte;
    	tempturn=xte*KP;
		tempturn=tempturn+i_xte*KI; 
    	tempturn=tempturn+d_xte*KD;
		if (tempturn > max_xte_nav_angle)
       		tempturn= max_xte_nav_angle;
    	if (tempturn < -max_xte_nav_angle)
       		tempturn= -max_xte_nav_angle;

    	cog_target=wp_course+tempturn; 
		
		if (cog_target>=360) 
   		   cog_target=cog_target-360;
 		if (cog_target<0) 
     		 cog_target=360+cog_target;
		
	} // if else
  	cog_error = cog_target - tempcog;
 	cog_error=limiterror(cog_error);
  
    turnrate=cog_error;
    if (turnrate>max_turnrate) 
    	turnrate=max_turnrate;
 	if (turnrate<-max_turnrate)
   		turnrate=-max_turnrate;
	// check if waypoint is reached
	if (distance_to_wp < wp[nextwp].arrival_dist)
		wp_next(2);

	return (turnrate);
} // navigate
